#include <assert.h>

#include <iostream>

#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoRotationXYZ.h>
#include "cutter.hpp"

using std::cout;
using std::endl;

cylinder_cutter::cylinder_cutter(double diam,
				 double length,
				 double z_bottom)
{
  r=diam/2;
  ztop=length+z_bottom;
  zbot=z_bottom;
  
  orient=identity();
  inverse_orient=identity();

  calc_bbox();

  cyl=NULL;
  
}

void cylinder_cutter::calc_i_bbox()
{
  identity_bbox.clear();
  uvector p(4);

  p[3]=1; //refer to Yuschenko
  
  p[2]=zbot; //bootom plate
  //1
  p[0]=-r; p[1]=r;
  identity_bbox.push_back(p);
  //2
  p[0]=r; p[1]=r;
  identity_bbox.push_back(p);
  //3
  p[0]=r; p[1]=-r;
  identity_bbox.push_back(p);
  //4
  p[0]=-r; p[1]=-r;
  identity_bbox.push_back(p);

  p[2]=ztop; //top plate
  //1
  p[0]=-r; p[1]=r;
  identity_bbox.push_back(p);
  //2
  p[0]=r; p[1]=r;
  identity_bbox.push_back(p);
  //3
  p[0]=r; p[1]=-r;
  identity_bbox.push_back(p);
  //4
  p[0]=-r; p[1]=-r;
  identity_bbox.push_back(p);
}

void cylinder_cutter::calc_bbox()
{
  if(identity_bbox.size()!=8)
    calc_i_bbox();
  bbox.clear();
  for(int i=0; i<8; i++)
    {
      uvector p=prod(orient,identity_bbox[i]);
      bbox.push_back(p);
    }
}

double cylinder_cutter::max_bbox_ind(int ind)
{
  double v=bbox[0](ind);

  for(int i=1; i<8; i++)
    if(bbox[i](ind)>v)
      v=bbox[i](ind);

  return v;
}

double cylinder_cutter::min_bbox_ind(int ind)
{
  double v=bbox[0](ind);

  for(int i=1; i<8; i++)
    if(bbox[i](ind)<v)
      v=bbox[i](ind);

  return v;
}

void cylinder_cutter::set_orient(matr T){

  orient=T;
  inverse_orient=special_inverse(T);

  
   if(cyl!=NULL)
       trans->matrix.setValue(orient(0,0),orient(1,0),orient(2,0),orient(3,0),
			 orient(0,1),orient(1,1),orient(2,1),orient(3,1),
			 orient(0,2),orient(1,2),orient(2,2),orient(3,2),
			 orient(0,3),orient(1,3),orient(2,3),orient(3,3));
    
  calc_bbox();

}

interval cylinder_cutter::intersect_z(double x0, double y0)
{
  if(x0<xmin() || x0>xmax() ||
     y0<ymin() || y0>ymax())
    return interval();

  //ray origin
  uvector o(4);
  o[0]=x0; o[1]=y0; o[2]=zmin(); o[3]=1;
  
  
  //ray direction
  uvector d(4);
  d[0]=0; d[1]=0; d[2]=1; d[3]=0;

  //transform ray into local CS
  o=prod(inverse_orient,o);
  d=prod(inverse_orient,d);
  
  x0=o[0]; y0=o[1];
  double z0=o[2];

  double i=d[0], j=d[1], k=d[2];

  double
    a=i*i+j*j,
    b=2*i*x0+2*j*y0,
    c=x0*x0+y0*y0-r*r;

  double D=b*b-4*a*c;

  vector<double> hits;
  
  if(D>=0)
    {
      double t1=(-b+sqrt(D))/2/a;
      double z=k*t1+z0;
      if(z>zbot && z<ztop)
	hits.push_back(t1+zmin());

      double t2=(-b-sqrt(D))/2/a;
      z=k*t2+z0;
      if(z>zbot && z<ztop && t1!=t2)
	hits.push_back(t2+zmin());
    }
  if(hits.size()==2)
    {
    	  
	  
      if(hits[0]<hits[1])
	return interval(hits[0],hits[1]);
      else
      	return interval(hits[1],hits[0]);
    }
  
  double t=(ztop-z0)/k;
  double x=i*t+x0, y=j*t+y0;
  if(x*x+y*y<r*r)
    hits.push_back(t+zmin());

  t=(zbot-z0)/k;
  x=i*t+x0, y=j*t+y0;
  if(x*x+y*y<r*r)
    hits.push_back(t+zmin());

  if(hits.size()==2)
    {
      if(hits[0]<hits[1])
	return interval(hits[0],hits[1]);
      else
      	return interval(hits[1],hits[0]);
    }

  return interval();
}


SoSeparator* cylinder_cutter::getview()
{
  if(cyl!=NULL) return cyl;
   
  cyl=new SoSeparator;
  cyl->ref();
  
  
  trans=new SoMatrixTransform;

  trans->matrix.setValue(orient(0,0),orient(1,0),orient(2,0),orient(3,0),
			 orient(0,1),orient(1,1),orient(2,1),orient(3,1),
			 orient(0,2),orient(1,2),orient(2,2),orient(3,2),
			 orient(0,3),orient(1,3),orient(2,3),orient(3,3));
  
  SoTranslation *heal=new SoTranslation;

   heal->translation.setValue(0,0,zbot+(ztop-zbot)/2);


   SoRotationXYZ* ro= new SoRotationXYZ;

   ro->angle.setValue(3.1415/2);
   
  SoCylinder *c= new SoCylinder;
  c->radius.setValue(r);
  c->height.setValue(ztop-zbot);

  
  cyl->addChild(trans);
  cyl->addChild(heal);
  cyl->addChild(ro);
  
  cyl->addChild(c);

  return cyl;
}

///////////////////////////////////////////////////////////////////////////

void  vertical_cylinder::set_orient(matr T){

  orient=T;

  orient(0,0)=1;
  orient(1,1)=1;
  orient(2,2)=1;
  orient(3,3)=1;

  orient(0,1)=orient(1,0)=0;
  orient(0,2)=orient(2,0)=0;
  orient(1,2)=orient(2,1)=0;


  xc=orient(0,3);
  yc=orient(1,3);
  zc=orient(2,3);
  
  
   
   if(cyl!=NULL)
       trans->matrix.setValue(orient(0,0),orient(1,0),orient(2,0),orient(3,0),
			 orient(0,1),orient(1,1),orient(2,1),orient(3,1),
			 orient(0,2),orient(1,2),orient(2,2),orient(3,2),
			 orient(0,3),orient(1,3),orient(2,3),orient(3,3));
    
  

}

vertical_cylinder::vertical_cylinder(double diam, double length, double z_bottom)
{
  xc=yc=zc=0;
  zbot=z_bottom;
  ztop=length+z_bottom;

  orient=identity();
	cyl=NULL;

  r=diam/2;
}



interval vertical_cylinder::intersect_z(double x0, double y0)
{
  if((x0-xc)*(x0-xc)+(y0-yc)*(y0-yc)<=r*r)
    return interval(zc+zbot,zc+ztop);
  else
    return interval();
}

    


  
